#include "i2c_interface.hpp"
#include "esp_log.h"
#include "esp_check.h"
#define TAG "I2C_INTERFACE"

I2CInterface::I2CInterface()
    : i2c_num(I2C_NUM_0)
{
    // Default constructor, no initialization needed
}

I2CInterface::I2CInterface(i2c_port_t i2c_num, uint32_t scl_pin, uint32_t sda_pin, i2c_mode_t mode)
    : i2c_num(i2c_num)
{
    init(i2c_num, scl_pin, sda_pin, mode);
}

void I2CInterface::init(i2c_port_t i2c_num, uint32_t scl_pin, uint32_t sda_pin, i2c_mode_t mode)
{

    i2c_master_bus_config_t i2c_bus_config = {
        .i2c_port = i2c_num,
        .sda_io_num = (gpio_num_t)sda_pin,
        .scl_io_num = (gpio_num_t)scl_pin,
        .clk_source = I2C_CLK_SRC_DEFAULT,
        .glitch_ignore_cnt = 7,
    };
    ESP_ERROR_CHECK(i2c_new_master_bus(&i2c_bus_config, &this->bus_handle));
}

I2CInterface::~I2CInterface()
{
    i2c_mas(i2c_num);
}


esp_err_t i2c_eeprom_write(i2c_eeprom_handle_t eeprom_handle, uint32_t address, const uint8_t *data, uint32_t size)
{
    ESP_RETURN_ON_FALSE(eeprom_handle, ESP_ERR_NO_MEM, TAG, "no mem for buffer");
    for (int i = 0; i < eeprom_handle->addr_wordlen; i++) {
        eeprom_handle->buffer[i] = (address & (0xff << ((eeprom_handle->addr_wordlen - 1 - i) * 8))) >> ((eeprom_handle->addr_wordlen - 1 - i) * 8);
    }
    memcpy(eeprom_handle->buffer + eeprom_handle->addr_wordlen, data, size);

    return i2c_master_transmit(eeprom_handle->i2c_dev, eeprom_handle->buffer, eeprom_handle->addr_wordlen + size, -1);
}

esp_err_t i2c_eeprom_read(i2c_eeprom_handle_t eeprom_handle, uint32_t address, uint8_t *data, uint32_t size)
{
    ESP_RETURN_ON_FALSE(eeprom_handle, ESP_ERR_NO_MEM, TAG, "no mem for buffer");
    for (int i = 0; i < eeprom_handle->addr_wordlen; i++) {
        eeprom_handle->buffer[i] = (address & (0xff << ((eeprom_handle->addr_wordlen - 1 - i) * 8))) >> ((eeprom_handle->addr_wordlen - 1 - i) * 8);
    }

    return i2c_master_transmit_receive(eeprom_handle->i2c_dev, eeprom_handle->buffer, eeprom_handle->addr_wordlen, data, size, -1);
}

int32_t I2CInterface::write(i2c_master_dev_handle_t i2c_dev, uint16_t addr, const uint8_t *data, uint32_t length)
{
    ESP_RETURN_ON_FALSE(data, ESP_ERR_NO_MEM, TAG, "no mem for buffer");
    for (int i = 0; i < 2; i++) {
        eeprom_handle->buffer[i] = (address & (0xff << ((eeprom_handle->addr_wordlen - 1 - i) * 8))) >> ((eeprom_handle->addr_wordlen - 1 - i) * 8);
    }
    memcpy(eeprom_handle->buffer + eeprom_handle->addr_wordlen, data, size);

    return i2c_master_transmit(eeprom_handle->i2c_dev, eeprom_handle->buffer, eeprom_handle->addr_wordlen + size, -1);
    
    // i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    // i2c_master_start(cmd);
    // i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    // i2c_master_write(cmd, data, length, true);
    // i2c_master_stop(cmd);
    // esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    // i2c_cmd_link_delete(cmd);
    // return ret == ESP_OK ? length : -1;
}

int32_t I2CInterface::write(i2c_master_dev_handle_t i2c_dev, const uint8_t *data, uint32_t length)
{
    return i2c_master_transmit(i2c_dev, data,  length, -1);
}

int32_t I2CInterface::writeToDevice(uint16_t addr, const void *reg_data, size_t reg_size, const uint8_t *data, uint32_t length)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(cmd, static_cast<const uint8_t*>(reg_data), reg_size, true);
    i2c_master_write(cmd, data, length, true);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    return ret == ESP_OK ? length : -1;
}

int32_t I2CInterface::read(uint16_t addr, uint8_t *data, uint32_t length)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, data, length, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    return ret == ESP_OK ? length : -1;
}

int32_t I2CInterface::readFromDevice(uint16_t addr, const void *reg_data, size_t reg_size, uint8_t *data, uint32_t length)
{
    i2c_cmd_handle_t cmd = i2c_cmd_link_create();
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_WRITE, true);
    i2c_master_write(cmd, static_cast<const uint8_t*>(reg_data), reg_size, true);
    i2c_master_start(cmd);
    i2c_master_write_byte(cmd, (addr << 1) | I2C_MASTER_READ, true);
    i2c_master_read(cmd, data, length, I2C_MASTER_LAST_NACK);
    i2c_master_stop(cmd);
    esp_err_t ret = i2c_master_cmd_begin(i2c_num, cmd, pdMS_TO_TICKS(1000));
    i2c_cmd_link_delete(cmd);
    return ret == ESP_OK ? length : -1;
}
